/* ========================================================================
 * PROJECT: FTIR Blob Tracker
 * ========================================================================
 * This work is based on the original FTIR Blob Tracker developed by
 *   Woonhyuk Baek
 *   Woontack Woo
 *   U-VR Lab, GIST of Gwangju in Korea.
 *   
 *   http://uvr.gist.ac.kr/
 *
 * Copyright of the derived and new portions of this work
 *     (C) 2009 GIST U-VR Lab.
 *
 * This framework is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This framework is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this framework; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * For further information please contact 
 *   Woonhyuk Baek
 *   <windage@live.com>
 *   GIST U-VR Lab.
 *   Department of Information and Communication
 *   Gwangju Institute of Science and Technology
 *   1, Oryong-dong, Buk-gu, Gwangju
 *   South Korea
 * ========================================================================
 ** @author   Woonhyuk Baek
 * ======================================================================== */

#include "Tracker.h"
#include <list>

int trackKnn(Blob blob, std::vector<Blob> *tracked, unsigned int k, double maxDistance = 50, double thresh = 0)
{

	int winner = -1; //initially label track as '-1'=dead
	if((k%2)==0) k++; //if k is not an odd number, add 1 to it

	//if it exists, square the threshold to use as square distance
	if(thresh>0)
		thresh *= thresh;

	std::list<std::pair<int,double> > nbors;
	std::list<std::pair<int,double> >::iterator iter;

	//find '2' closest neighbors of testpoint
	double x, y, xT, yT, dist;
	for(unsigned int i=0; i<tracked->size(); i++)
	{
		x = (*tracked)[i].pt.x;
		y = (*tracked)[i].pt.y;

		xT = blob.pt.x;
		yT = blob.pt.y;
		dist = (x-xT)*(x-xT)+(y-yT)*(y-yT);

		if(dist<=thresh)//it's good, apply label if no label yet and return
		{
			winner = i;
			return winner;
		}

		/****************************************************************
		* check if this blob is closer to the point than what we've seen
		*so far and add it to the index/distance list if positive
		****************************************************************/

		//search the list for the first point with a longer distance
		for(iter=nbors.begin(); iter!=nbors.end()
			&& dist>=iter->second; iter++);

		if((iter!=nbors.end())||(nbors.size()<k)) //it's valid, insert it
		{
			nbors.insert(iter, 1, std::pair<int, double>(i, dist));
			//too many items in list, get rid of farthest neighbor
			if(nbors.size()>k)
				nbors.pop_back();
		}
	}

	for(iter=nbors.begin(); iter!=nbors.end(); iter++)
	{
		// WANNING! i don't know this magic number
		if(iter->second < maxDistance*maxDistance) {
			winner = iter->first;
		}
	}

	return winner;
}


void BlobTracker::Do()
{
	//initialize ID's of all blobs
	removedBlobList.clear();
	for(unsigned int i=0; i<newBlobList.size(); i++)
		newBlobList[i].id=-1;

	//go through all tracked blobs to compute nearest new point
	for(unsigned int i=0; i<trackedBlobList.size(); i++)
	{
		int winner = trackKnn(trackedBlobList[i], &newBlobList, 1, MAX_MOVEMENT_THRESHOLD, 0);
		if(winner==-1) //track has died, mark it for deletion
		{
			// TODO SEND BLOB OFF MESSAGE
			trackedBlobList[i].eventMessage = Blob::MEMSSAGE_REMOVE;
			removedBlobList.push_back(trackedBlobList[i]);

			trackedBlobList[i].id = -1;
		}
		else //still alive, have to update
		{
			if(newBlobList[winner].id!=-1)
			{
				//find the currently assigned blob
				unsigned int j; //j will be the index of it
				for(j=0; j<trackedBlobList.size(); j++)
				{
					if(trackedBlobList[j].id==newBlobList[winner].id)
						break;
				}

				if(j==trackedBlobList.size())//got to end without finding it
				{
					newBlobList[winner].id = trackedBlobList[i].id;
					newBlobList[winner].age = trackedBlobList[i].age;
					newBlobList[winner].color = trackedBlobList[i].color;
					newBlobList[winner].sitting = trackedBlobList[i].sitting;
					newBlobList[winner].downTime = trackedBlobList[i].downTime;
					newBlobList[winner].lastTimeTimeWasChecked = trackedBlobList[i].lastTimeTimeWasChecked;

					trackedBlobList[i] = newBlobList[winner];
				}
				else //found it, compare with current blob
				{
					double x = newBlobList[winner].pt.x;
					double y = newBlobList[winner].pt.y;
					double xOld = trackedBlobList[j].pt.x;
					double yOld = trackedBlobList[j].pt.y;
					double xNew = trackedBlobList[i].pt.x;
					double yNew = trackedBlobList[i].pt.y;
					double distOld = (x-xOld)*(x-xOld)+(y-yOld)*(y-yOld);
					double distNew = (x-xNew)*(x-xNew)+(y-yNew)*(y-yNew);

					//if this track is closer, update the ID of the blob
					//otherwise delete this track.. it's dead
					if(distNew<distOld) //update
					{
						newBlobList[winner].id = trackedBlobList[i].id;
						newBlobList[winner].age = trackedBlobList[i].age;
						newBlobList[winner].color = trackedBlobList[i].color;
						newBlobList[winner].sitting = trackedBlobList[i].sitting;
						newBlobList[winner].downTime = trackedBlobList[i].downTime;
						newBlobList[winner].lastTimeTimeWasChecked = trackedBlobList[i].lastTimeTimeWasChecked;

						// TODO SEND BLOB OFF MESSAGE
						trackedBlobList[j].eventMessage = Blob::MEMSSAGE_REMOVE;
						removedBlobList.push_back(trackedBlobList[j]);

						trackedBlobList[j].id = -1;
					}
					else //delete
					{
						// TODO SEND BLOB OFF MESSAGE
						trackedBlobList[i].eventMessage = Blob::MEMSSAGE_REMOVE;
						removedBlobList.push_back(trackedBlobList[i]);

						trackedBlobList[i].id = -1;
					}
				}
			}
			else //no conflicts, so simply update
			{
				newBlobList[winner].id = trackedBlobList[i].id;
				newBlobList[winner].age = trackedBlobList[i].age;
				newBlobList[winner].color = trackedBlobList[i].color;
				newBlobList[winner].sitting = trackedBlobList[i].sitting;
				newBlobList[winner].downTime = trackedBlobList[i].downTime;
				newBlobList[winner].lastTimeTimeWasChecked = trackedBlobList[i].lastTimeTimeWasChecked;
			}

		}
	}

	//--Update All Current Tracks
	//remove every track labeled as dead (ID='-1')
	//find every track that's alive and copy it's data from newBlobs
	for(unsigned int i=0; i<trackedBlobList.size(); i++)
	{
		if(trackedBlobList[i].id==-1) //dead
		{
			//erase track
			trackedBlobList.erase(trackedBlobList.begin()+i);
			i--; //decrement one since we removed an element
		}
		else //living, so update it's data
		{
			for(unsigned int j=0; j<newBlobList.size(); j++)
			{
				if(trackedBlobList[i].id==newBlobList[j].id)
				{
					//update track
					BlobPoint tempLastPt = trackedBlobList[i].pt;
					trackedBlobList[i] = newBlobList[j];
					trackedBlobList[i].lastPt = tempLastPt;

					double currentTime = cvGetTickCount()/(cvGetTickFrequency() * 1000);
					double dt = (currentTime - trackedBlobList[i].lastTimeTimeWasChecked);
					double dx = (trackedBlobList[i].pt.x - trackedBlobList[i].lastPt.x)/dt;
					double dy = (trackedBlobList[i].pt.y - trackedBlobList[i].lastPt.y)/dt;
					trackedBlobList[i].maccel = sqrt((dx*dx + dy*dy));
					trackedBlobList[i].lastTimeTimeWasChecked = currentTime;
					trackedBlobList[i].age = currentTime - trackedBlobList[i].downTime;

					//if not moving more than min_movement_threshold then set to same position as last frame
		            if(trackedBlobList[i].maccel < MIN_MOVEMENT_THRESHOLD)
					{	//this helps avoid jittery blobs
						trackedBlobList[i].pt.x = trackedBlobList[i].lastPt.x;
						trackedBlobList[i].pt.y = trackedBlobList[i].lastPt.y;
                    }

					//set sitting (held length)
                    if(trackedBlobList[i].maccel < MIN_MOVEMENT_THRESHOLD*2)
					{	//1 more frame of sitting
						if(trackedBlobList[i].sitting != -1)
						trackedBlobList[i].sitting = currentTime - trackedBlobList[i].downTime;           
					}
					else {
						trackedBlobList[i].sitting = -1;
					}

					//if blob has been 'holding/sitting' for 1 second send a held event
					if(trackedBlobList[i].sitting > 1000.0f)
					{
						// TODO SEND BLOB HELD EVENT
						trackedBlobList[i].eventMessage = Blob::MEMSSAGE_HOLD;
					}
					else
					{
						// TODO SEND BLOB MOVED EVENT
						trackedBlobList[i].eventMessage = Blob::MEMSSAGE_MOVE;
					}
				}
			}
		}
	}

	//--Add New Living Tracks
	//now every new blob should be either labeled with a tracked ID or\
	//have ID of -1... if the ID is -1... we need to make a new track
	for(unsigned int i=0; i<newBlobList.size(); i++)
	{
		if(newBlobList[i].id==-1)
		{
			double currentTime = cvGetTickCount()/(cvGetTickFrequency() * 1000);
			CvRNG rng = cvRNG(cvGetTickCount());

			newBlobList[i].id = IDCounter++;
			newBlobList[i].downTime = currentTime;
			newBlobList[i].color = CV_RGB(cvRandInt(&rng)%150+56, cvRandInt(&rng)%150+56, cvRandInt(&rng)%150+56);

			// TODO SEND ADD BLOB EVENT
			newBlobList[i].eventMessage = Blob::MEMSSAGE_ADD;

			trackedBlobList.push_back(newBlobList[i]);
		}
	}

	this->ReleaseNewBlob();
}